import Facility from "../facility/Facility";
import { Location } from "../info/Predefine";

type Point = {
    x: number;
    y: number;
    g?: number; // Cost from start Point to the current Point
    h?: number; // Heuristic cost from current Point to end Point
    f?: number; // Total cost (f = g + h)
    parent?: { x: number; y: number }; // Previous Point
};

class PriorityQueue {
    public elements: { Point: Point; f: number }[] = [];

    constructor() { }

    enqueue(Point: Point, f: number) {
        this.elements.push({ Point, f });
        this.elements.sort((a, b) => a.f - b.f); // keep the queue sorted
    }

    dequeue(): Point | undefined {
        return this.elements.length ? this.elements.shift()!.Point : undefined;
    }

    isEmpty(): boolean {
        return this.elements.length === 0;
    }
}

function heuristic(a: Point, b: Point): number {
    // Using Manhattan distance
    return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
}

function inBounds(x: number, y: number, width: number, height: number): boolean {
    return x >= 0 && x < width && y >= 0 && y < height;
}

function passable(Point: Point, grid: Facility[][]): boolean {
    return true
}

export function astar(grid: Facility[][], startLocation: Location, endLocation: Location): Location[] {
    const startX = startLocation.locationY;
    const startY = startLocation.locationX;
    const endX = endLocation.locationY;
    const endY = endLocation.locationX;
    const start: Point = { x: startX, y: startY, g: 0, h: 0, f: 0 };
    const end: Point = { x: endX, y: endY, g: 0, h: 0, f: 0 };
    const openList = new PriorityQueue();
    const closedList: Set<string> = new Set();

    openList.enqueue(start, start.h = heuristic(start, end));

    while (!openList.isEmpty()) {
        const currentPoint = openList.dequeue()!;

        if (currentPoint.x === end.x && currentPoint.y === end.y) {
            return reconstructPath(currentPoint, start); // Return path with the start node at the end
        }

        closedList.add(`${currentPoint.x},${currentPoint.y}`);

        for (let i = -1; i <= 1; i++) {
            for (let j = -1; j <= 1; j++) {
                if ((i === 0 && j !== 0) || (i !== 0 && j === 0)) {
                    if (i === 0 && j === 0) continue; // Skip the current Point

                    const neighbor: Point = { x: currentPoint.x + i, y: currentPoint.y + j };
                    if (!inBounds(neighbor.x, neighbor.y, grid.length, grid[0].length) || !passable(neighbor, grid)) continue;
                    if (closedList.has(`${neighbor.x},${neighbor.y}`)) continue;

                    const tentativeG = currentPoint.g + 1; // Assuming uniform cost between Points
                    let isNewPoint = !openList.elements.some(e => neighbor.x === e.Point.x && neighbor.y === e.Point.y);

                    if (isNewPoint) {
                        openList.enqueue(neighbor, heuristic(neighbor, end) + tentativeG);
                    } else if (tentativeG < neighbor.g) {
                        // Found a shorter path to the neighbor
                        neighbor.g = tentativeG;
                        openList.enqueue(neighbor, neighbor.f = neighbor.g + heuristic(neighbor, end));
                    }

                    if (isNewPoint) {
                        neighbor.h = heuristic(neighbor, end);
                        neighbor.parent = currentPoint;
                    }
                }
            }
        }
    }

    return []; // No path found
}

function reconstructPath(currentNode: Point, startNode: Point): Location[] {
    const path: Location[] = [];
    // 从当前节点回溯到起点，构建路径
    while (currentNode) {
        path.unshift({ locationX: currentNode.y, locationY: currentNode.x }); 
        currentNode = currentNode.parent;
    }
    // // 将起始节点添加到路径数组的最后
    // path.push({ locationX: startNode.x, locationY: startNode.y });
    return path;
}